module FES1DL22_m
use FES1DL2_m
use Geometry_m

type, extends(FES1DL2) :: FES1DL22
contains
    procedure :: initFES => initFES1DL22
    
    procedure :: assembleEulerBeamStiffL22_func
    procedure :: assembleEulerBeamStiffL22_const
    generic :: assembleEulerBeamStiff => assembleEulerBeamStiffL22_func, assembleEulerBeamStiffL22_const

    procedure :: assembleTimoshenkoBeamStiffL22_func
    procedure :: assembleTimoshenkoBeamStiffL22_const
    generic :: assembleTimoshenkoBeamStiff => assembleTimoshenkoBeamStiffL22_func, assembleTimoshenkoBeamStiffL22_const
    
!    procedure :: assembleMixedBeamStiffL22_func
!    procedure :: assembleMixedBeamStiffL22_const
!    generic :: assembleTimoshenkoBeamStiff => assembleMixedBeamStiffL22_func, assembleMixedBeamStiffL22_const
    
    procedure :: assembleEulerPressureL22_func
    procedure :: assembleEulerPressureL22_const
    generic :: assembleEulerPressure => assembleEulerPressureL22_func, assembleEulerPressureL22_const
    
    procedure :: assembleLumpForceL22_const
    generic :: assembleLumpForce => assembleLumpForceL22_const
    
    procedure :: assembleTimoshenkoPressureL22_func
    procedure :: assembleTimoshenkoPressureL22_const
    generic :: assembleTimoshenkoPressure => assembleTimoshenkoPressureL22_func, assembleTimoshenkoPressureL22_const
    
    procedure :: applyBCZeroL22_MR
    procedure :: applyBCZeroL22_M
    procedure :: applyBCZeroL22_R
    generic :: applyBCZero => applyBCZeroL22_MR, applyBCZeroL22_M, applyBCZeroL22_R
    
    procedure :: applyBCL22_const
    procedure :: applyBCL22_point
    generic :: applyBC => applyBCL22_const, applyBCL22_point
end type

integer, parameter :: EULER = 1, TIMOSHENKO = 2

contains

subroutine initFES1DL22(this, mesh1D)
    implicit none
    class(FES1DL22), intent(out) :: this
    type(mesh1DL2), target, intent(in) :: mesh1D

    integer :: band
    this%dim = 1
    this%nDof = mesh1D%nv * 2
    this%nVertices = mesh1D%nv
    this%nElement = mesh1D%nt
    this%nBoundary = mesh1D%nb
    this%nPerElement = 2
    this%nPerBoundary = 1
    this%dofPerNode = 2
    this%dofPerElement = 4
    this%dofPerBoundary = 2
    
    band = mesh1D.elementDiff()
    this%nBand = (band + 1) * 2
    this%mesh1D => mesh1D
end subroutine

subroutine assembleEulerBeamStiffL22_func(this, funcEI, time, tp, A)
    implicit none
    class(FES1DL22), intent(in) :: this
    real*8, external :: funcEI
    real*8, intent(in) :: time
    integer, intent(in) :: tp
    type(smatrix), intent(inout):: A
    
    call this.assembleGlobalMatrix_function(funcEI, time, EulerBeamStiffL22, tp, A)
end subroutine

subroutine assembleEulerBeamStiffL22_const(this, constEI, tp, A)
    implicit none
    class(FES1DL22), intent(in) :: this
    real*8, intent(in) :: constEI
    integer, intent(in) :: tp
    type(smatrix), intent(inout):: A
    
    call this.assembleGlobalMatrix_const(constEI, EulerBeamStiffL22, tp, A)
end subroutine

subroutine EulerBeamStiffL22(x, EI, tp, K)
    implicit none
    real*8, intent(in) :: x(2), EI(2)
    integer, intent(in) :: tp
    real*8, intent(out) :: K(4, 4)
    
    real*8 :: l, mEI
    l = abs(x(2)-x(1))
    mEI = 0.5D0 * sum(EI)
    K(1, 1) = 12
    K(1, 2) = 6 * l
    K(1, 3) = -12
    K(1, 4) = 6 * l
    K(2, 1) = K(1, 2)
    K(2, 2) = 4 * l * l
    K(2, 3) = -6 * l
    K(2, 4) = 2 * l * l
    K(3, 1) = K(1, 3)
    K(3, 2) = K(2, 3)
    K(3, 3) = 12
    K(3, 4) = -6 * l
    K(4, 1) = K(1, 4)
    K(4, 2) = K(2, 4)
    K(4, 3) = K(3, 4)
    K(4, 4) = 4 * l * l
    K = K * mEI / l**3
end subroutine

subroutine assembleTimoshenkoBeamStiffL22_func(this, funcEI, funcKsGA, time, tp, A)
    implicit none
    class(FES1DL22), intent(in) :: this
    real*8, external :: funcEI, funcKsGA
    real*8, intent(in) :: time
    integer, intent(in) :: tp
    type(SMatrix), intent(inout):: A

    call this.assembleGlobalMatrix_function2(funcEI, funcKsGA, time, TimoshenkoBeamStiffL22, tp, A)    
end subroutine

subroutine assembleTimoshenkoBeamStiffL22_const(this, constEI, constKsGA, tp, A)
    implicit none
    class(FES1DL22), intent(in) :: this
    real*8, intent(in) :: constEI, constKsGA
    integer, intent(in) :: tp
    type(SMatrix), intent(inout):: A

	call this.assembleGlobalMatrix_const2(constEI, constKsGA, TimoshenkoBeamStiffL22, tp, A)    
end subroutine


subroutine TimoshenkoBeamStiffL22(x, EI, KsGA, tp, K)
    implicit none
    real*8, intent(in) :: x(2), EI(2), KsGA(2)
    integer, intent(in) :: tp
    real*8, intent(out) :: K(4, 4)
    
    real*8 :: length, mEI, mksGA, Kb(4, 4), KKs(4, 4)
    length = abs(x(2) - x(1))
    mEI = 0.5D0 * sum(EI)
    mksGA = 0.5D0 * sum(KsGA)
    
    Kb = 0.0D0
    Kb(2, 2) = mEI / length
    Kb(2, 4) = -Kb(2, 2)
    Kb(4, 2) = Kb(2, 4)
    Kb(4, 4) = Kb(2, 2)
    
    KKs(1, 1) = mksGA / length
    KKs(1, 2) = 0.5D0 * mksGA
    KKs(1, 3) = -mksGA / length
    KKs(1, 4) = KKs(1, 2)
    KKs(2, 1) = KKs(1, 2)
    KKs(2, 2) = 0.25D0 * mksGA * length
    KKs(2, 3) = -KKs(1, 2)
    KKs(2, 4) = KKs(2, 2)
    KKs(3, 1) = KKs(1, 3)
    KKs(3, 2) = KKs(2, 3)
    KKs(3, 3) = KKs(1, 1)
    KKs(3, 4) = KKs(3, 2)
    KKs(4, 1) = KKs(1, 4)
    KKs(4, 2) = KKs(2, 4)
    KKs(4, 3) = KKs(3, 4)
    KKs(4, 4) = KKs(2, 2)
    
    K = Kb + KKs
end subroutine

subroutine assembleLumpForceL22_const(this, constP, node, RHS)
    implicit none
    class(FES1DL22), intent(in) :: this
    real*8, intent(in) :: constP
    integer, intent(in) :: node
    real*8, intent(inout):: RHS(:)
    
    integer :: idx
    idx = 2 * node - 1
    RHS(idx) = RHS(idx) + constP
end subroutine
    
subroutine assembleEulerPressureL22_func(this, funcP, time, tp, RHS)
    implicit none
    class(FES1DL22), intent(in) :: this
    real*8, external :: funcP
    real*8, intent(in) :: time
    integer, intent(in) :: tp
    real*8, intent(inout):: RHS(:)
    
    call this.assembleGlobalVector_function(funcP, time, elementEulerPressureL22, tp, RHS)
end subroutine


subroutine assembleEulerPressureL22_const(this, constP, tp, RHS)
    implicit none
    class(FES1DL22), intent(in) :: this
    real*8, intent(in) :: constP
    integer, intent(in) :: tp
    real*8, intent(inout):: RHS(:)
    
    call this.assembleGlobalVector_const(constP, elementEulerPressureL22, tp, RHS)
end subroutine

subroutine elementEulerPressureL22(x, P, tp, K)
    implicit none
    real*8, intent(in) :: x(2), P(2)
    integer, intent(in) :: tp
    real*8, intent(out) :: K(4)
    
    real*8 :: length, mp
    length = abs(x(2) - x(1))
    mp = 0.5D0 * sum(P)
    
    K(1) = 0.5D0 * mp * length
    K(2) = mp * length**2 / 12.0D0
    K(3) = K(1)
    K(4) = -K(2)
end subroutine

subroutine assembleTimoshenkoPressureL22_func(this, funcP, time, tp, RHS)
    implicit none
    class(FES1DL22), intent(in) :: this
    real*8, external :: funcP
    real*8, intent(in) :: time
    integer, intent(in) :: tp
    real*8, intent(inout):: RHS(:)
    
    call this.assembleGlobalVector_function(funcP, time, elementTimoshenkoPressureL22, tp, RHS)
end subroutine

subroutine assembleTimoshenkoPressureL22_const(this, constP, tp, RHS)
    implicit none
    class(FES1DL22), intent(in) :: this
    real*8, intent(in) :: constP
    integer, intent(in) :: tp
    real*8, intent(inout):: RHS(:)
    
    call this.assembleGlobalVector_const(constP, elementTimoshenkoPressureL22, tp, RHS)
end subroutine

subroutine elementTimoshenkoPressureL22(x, P, tp, K)
    implicit none
    real*8, intent(in) :: x(2), P(2)
    integer, intent(in) :: tp
    real*8, intent(out) :: K(4)
    
    real*8 :: length, mp
    length = abs(x(2) - x(1))
    mp = 0.5D0 * sum(P)
    
    K(1) = 0.5D0 * mp * length
    K(2) = 0.0D0
    K(3) = K(1)
    K(4) = 0.0D0
end subroutine

subroutine applyBCZeroL22_MR(this, A, RHS, label, direct)
    implicit none
    class(FES1DL22), intent(in) :: this
    type(smatrix), intent(inout):: A
    real*8, intent(inout) :: RHS(A%M)
    integer, intent(in) :: label(:)
    integer, intent(in), optional :: direct

    type(LinkedSet) :: dirichletBC
    call this.mesh1D.extractBoundaryNodes(dirichletBC, label)
    if (present(direct)) then
        call this.applyBCZero0_MR(A, RHS, dirichletBC, direct)
    else
        call this.applyBCZero0_MR(A, RHS, dirichletBC)
    end if
	call dirichletBC.empty()
end subroutine


subroutine applyBCZeroL22_M(this, A, label, direct)
    use Set_m
	implicit none
	class(FES1DL22), intent(in) :: this  
	type(SMatrix), intent(inout):: A  
	integer, intent(in) :: label(:)
    integer, intent(in), optional :: direct

	type(mesh1DL2), pointer :: mesh1D
	type(LinkedSet) :: dirichletBC
	mesh1D => this%mesh1D
	call mesh1D.extractBoundaryNodes(dirichletBC, label)
    if (present(direct)) then
        call this.applyBCZero0_M(A, dirichletBC, direct)
    else
        call this.applyBCZero0_M(A, dirichletBC)
    end if
	call dirichletBC.empty()
end subroutine

subroutine applyBCZeroL22_R(this, RHS, label, direct)
    use Set_m
	implicit none
	class(FES1DL22), intent(in) :: this      
	real*8, intent(inout) :: RHS(:) 
	integer, intent(in) :: label(:)   
    integer, intent(in), optional :: direct

	type(mesh1DL2), pointer :: mesh1D
	type(LinkedSet) :: dirichletBC
	mesh1D => this%mesh1D
	call mesh1D.extractBoundaryNodes(dirichletBC, label)
    if (present(direct)) then
        call this.applyBCZero0_R(RHS, dirichletBC, direct)
    else
        call this.applyBCZero0_R(RHS, dirichletBC)
    end if
	call dirichletBC.empty()
end subroutine

subroutine applyBCL22_const(this, A, RHS, label, u0, v0, tp, direct)
    use Set_m
	implicit none
	class(FES1DL22), intent(in) :: this
	type(smatrix), intent(inout) :: A
	real*8, intent(inout) :: RHS(:)
	integer, intent(in) :: label(:)
	real*8, intent(in) :: u0, v0
	integer, intent(in) :: tp
    integer, intent(in), optional :: direct

	real*8, allocatable :: uu0(:)
	integer :: i
	real*8 :: x, y
	type(mesh1DL2), pointer :: mesh1D
    type(LinkedSet) :: dirichletBC
	type(ele), pointer:: current_p
    mesh1D => this%mesh1D

	call mesh1D.extractBoundaryNodes(dirichletBC, label)
    
    if (present(direct)) then
        allocate(uu0(dirichletBC%size))
        i = 0
        current_p => dirichletBC%head
        if (direct == directX) then
            uu0 = u0
        else if (direct == directY) then
            uu0 = v0
        end if
        call this.applyBC0(A, RHS, dirichletBC, uu0, tp, direct)
    else
        allocate(uu0(2 * dirichletBC%size))
        i = 0
        do i = 1, dirichletBC%size
            uu0(2 * i - 1) = u0
            uu0(2 * i) = v0
        end do
        call this.applyBC0(A, RHS, dirichletBC, uu0, tp)
    end if
	deallocate(uu0)
	call dirichletBC.empty()
end subroutine

subroutine applyBCL22_point(this, A, RHS, node, u, v, tp, direct)
    use Set_m
    implicit none
    class(FES1DL22), intent(in) :: this
    type(SMatrix), intent(inout) :: A
    real*8, intent(inout) :: RHS(this%nDof)
    real*8, intent(in) :: u, v
    integer, intent(in) :: node, tp
    integer, intent(in), optional :: direct
    
    real*8, allocatable :: uu(:)
    integer, allocatable  :: BCDOF(:)
    if (.NOT. present(direct)) then
        allocate(BCDOF(2), uu(2))
        BCDOF(1) = 2 * node - 1
        BCDOF(2) = 2 * node
        uu(1) = u
        uu(2) = v
        call this.applyBC_DOF(A, RHS, BCDOF, uu, tp)
    else if (direct == 1) then
        allocate(BCDOF(1), uu(1))
        BCDOF(1) = 2 * node - 1
        uu(1) = u
        call this.applyBC_DOF(A, RHS, BCDOF, uu, tp)
    else if (direct == 2) then
        allocate(BCDOF(1), uu(1))
        BCDOF(1) = 2 * node
        uu(1) = v
        call this.applyBC_DOF(A, RHS, BCDOF, uu, tp)
    end if
    deallocate(BCDOF, uu)
end subroutine

end module
    